package kit.edu.lego.gruppe3.levels.treeArea.old;

import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.actions.Movement;
import kit.edu.lego.gruppe3.actions.Orientation;
import kit.edu.lego.gruppe3.levels.LevelSelector;
import kit.edu.lego.gruppe3.levels.LevelSelector.Level;
import kit.edu.lego.gruppe3.sensors.Scanner;
import lejos.robotics.subsumption.Behavior;

public class CornerFinder implements Behavior {

	private double length = ComponentsRepository.VEHICLE_LENGTH;

	@Override
	public void action() {
		while (Scanner.getRangeAt(0) >= 30)
			Movement.travel(length / 2, 10);
		Orientation.orientAtWall();
		if (Scanner.getRangeAt(-90, true) < 30)
			TreeAreaSolver.setStatus(TreeAreaState.searchForTreeRight);
		else if (Scanner.getRangeAt(90, true) < 30)
			TreeAreaSolver.setStatus(TreeAreaState.searchForTreeLeft);
		else {
			Movement.turnCenter(90, true);
			while (Scanner.getRangeAt(0, true) >= 30) {
				Movement.travel(length / 2, 10);
			}
			Movement.turnCenter(90);
			TreeAreaSolver.setStatus(TreeAreaState.searchForTreeRight);
		}
	}

	@Override
	public void suppress() {
	}

	@Override
	public boolean takeControl() {
		return Level.treeArea == LevelSelector.getInstance().getCurrentLevel()
				&& TreeAreaState.searchForCorner == TreeAreaSolver.getStatus();
	}

}
